/*
  motion.h

   Licensed to the Apache Software Foundation (ASF) under one
   or more contributor license agreements.  See the NOTICE file
   distributed with this work for additional information
   regarding copyright ownership.  The ASF licenses this file
   to you under the Apache License, Version 2.0 (the
   "License"); you may not use this file except in compliance
   with the License.  You may obtain a copy of the License at

   http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing,
   software distributed under the License is distributed on an
   "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
   KIND, either express or implied.  See the License for the
   specific language governing permissions and limitations
   under the License.
 
   Created on: Aug 1, 2009
 
 */

#ifndef MOTION_H_
#define MOTION_H_

#include <stdlib.h>

#include <core/queues.h>

namespace crossEyed
{
namespace operators
{

using namespace core ;

template<class SOURCEFRAME, class PIXELADAPTER>
class TMotionFilter : public TManagedFrameOperator<TByteFrameBuffer>
{
protected:

   //
   // Input/Source: We get this from the preceding operator
   //
   TOperator<SOURCEFRAME> *     pSource ;
   TDataQueue<SOURCEFRAME> *    pSourceQueue ;

   //
   // These are the frames we will compute the deviation over.
   //
   TDataQueue<TByteFrameBuffer> tempFrameQueue ;

   //
   // We maintain one frame for the sum and the sum of squares
   // to efficiently compute the variation over.
   //
   struct THelperPixel {
      unsigned long sum ;
      unsigned long sumOfSquares ;
   };

   TFrameBuffer<THelperPixel>   helperFrame ;

   //
   // Output: The output frames are part of TOperator.
   //

public:

   TMotionFilter( TOperator<SOURCEFRAME> * pSource,
                  size_t N,
                  size_t tempN,
                  unsigned int width,
                  unsigned int height )
      : TManagedFrameOperator<TByteFrameBuffer>( N, width, height ),
        pSource( pSource ),
        pSourceQueue( pSource->getDataQueue() ),
        tempFrameQueue( tempN ),
        helperFrame( width, height )
   {
      //
      // Get frames to keep luminance information of the past tempN frames
      //
      for( unsigned int i = 0; i < tempN; i ++ )
      {
         TByteFrameBuffer *pBB ;
         try
         {
            pBB = new TByteFrameBuffer( width, height ); ;
         }
         catch(std::bad_alloc& badAlloc)
         {
            XI_OOM_HARD;
         }

         tempFrameQueue.swizzleWriterData( pBB ) ;

         //
         // The following method is somewhat overkill since it also deals with all kinds of
         // serialization issues. This is not necessary as this queue is written and read only from
         // within the operator thread.
         //
         tempFrameQueue.advanceWriterDataSlot() ;
      }

      //
      // Get one helper frame to cache some useful terms needed to incrementally maintain
      // the variance as we slide over frames.
      //

      //
      // At this point we can start grabbing frames from source to do some useful stuff.
      //
   }

   virtual ~TMotionFilter()
   {
      for( ;
           tempFrameQueue.getWriterData() != NULL ;
           tempFrameQueue.advanceWriterDataSlot() )
      {
         delete tempFrameQueue.getWriterData() ;

         tempFrameQueue.swizzleWriterData( NULL ) ;
      }
   }

   virtual void eventLoop()
   {
      unsigned int numProcessed = 0;
      BOOST_DEDUCED_TYPENAME TDataQueue<SOURCEFRAME>::TSubscription subscription ;

      THelperPixel *pHelperPixel ;

      while( !boost::this_thread::interruption_requested() )
      {
         try
         {
            pSourceQueue->renew( subscription ) ;
         }
         catch( boost::thread_interrupted interrupt )
         {
            break ;
         }

         //
         // Transfer image data over into the next frame - also update the helper frame
         //
         {
            TByteFrameBuffer * pBB ;
            unsigned char *    pDestPixel ;

            PIXELADAPTER pixelAdapter( subscription.pRep );

            pHelperPixel = this->helperFrame.getBuffer() ;

            pBB = tempFrameQueue.getWriterData() ;
            pDestPixel = pBB->getBuffer() ;

            for( unsigned int y = 0;
                     y < pBB->getHeight();
                     y ++ )
            {
               for( unsigned int x = 0;
                        x < pBB->getWidth(); )
               {
                  unsigned char pixelsInMacroPixel[PIXELADAPTER::getPixelsPerMacroPixel()];

                  pixelAdapter.toTarget( pixelsInMacroPixel ) ;

                  for( unsigned int m = 0;
                           m < PIXELADAPTER::getPixelsPerMacroPixel();
                           m ++, x ++)
                  {
                     if( numProcessed < this->tempFrameQueue.getN() )
                     {
                        if( numProcessed == 0 )
                        {
                           //
                           // bootstrapping if we haven't processed any frames yet
                           //
                           pHelperPixel->sum = pixelsInMacroPixel[m] ;
                           pHelperPixel->sumOfSquares = pixelsInMacroPixel[m] * pixelsInMacroPixel[m] ;
                        }
                        else
                        {
                           //
                           // bootstrapping if we haven't processed tempN frames yet
                           //
                           pHelperPixel->sum += pixelsInMacroPixel[m] ;
                           pHelperPixel->sumOfSquares += pixelsInMacroPixel[m] * pixelsInMacroPixel[m] ;
                        }
                     }
                     else
                     {
                        //
                        // non-bootstrapping
                        //

                        //
                        // subtract oldest frame - i.e. the one we are going to write into in a moment
                        //
                        pHelperPixel->sum -= ( *pDestPixel ) ;
                        pHelperPixel->sumOfSquares -= ( ( *pDestPixel ) * ( *pDestPixel ) ) ;

                        //
                        // add new frame
                        //
                        pHelperPixel->sum += pixelsInMacroPixel[m] ;
                        pHelperPixel->sumOfSquares += pixelsInMacroPixel[m] * pixelsInMacroPixel[m] ;
                     }

                     *pDestPixel = pixelsInMacroPixel[m] ;

                     pHelperPixel ++ ;
                     pDestPixel ++ ;
                  }

                  pixelAdapter++ ;
               }
            }

            //
            // Write out the temp frame - make sure we catch a possible interrupt
            // so we can give up the source subscription before we exit this method.
            //
            try
            {
               tempFrameQueue.writerDataFinishAndAdvance() ;
            }
            catch( boost::thread_interrupted interrupt )
            {
               break ;
            }

         }

         //
         // Now compute the output frame
         //
         if( numProcessed == this->tempFrameQueue.getN() )
         {
            unsigned char * pDestPixel ;

            pHelperPixel = this->helperFrame.getBuffer() ;
            pDestPixel   = this->frameQueue.getWriterData()->getBuffer() ;

            for( unsigned int y = 0;
                     y < this->helperFrame.getHeight();
                     y ++ )
            {
               for( unsigned int x = 0;
                        x < this->helperFrame.getWidth();
                        x ++ )
               {
                  double mean = (double)pHelperPixel->sum / (double)this->tempFrameQueue.getN() ;

                  *pDestPixel = (unsigned char )
                                    pow( ( (double)pHelperPixel->sumOfSquares / (double)this->tempFrameQueue.getN() ) -
                                         ( mean * mean ), (double)0.5 ) ;

                  pHelperPixel ++ ;
                  pDestPixel ++ ;
               }
            }
         }

         //
         // Write out the temp frame - make sure we catch a possible interrupt
         // so we can give up the source subscription before we exit this method.
         //
         try
         {
            this->frameQueue.writerDataFinishAndAdvance() ;
         }
         catch( boost::thread_interrupted interrupt )
         {
            break ;
         }

         if( numProcessed < this->tempFrameQueue.getN() )
         {
            numProcessed ++ ;
         }
      }

      if( subscription.pRep != NULL )
      {
         pSourceQueue->unsubscribe( subscription ) ;
      }
   }

};

template<class SOURCEFRAME, class PIXELADAPTER>
class TTemporalMotionFilter : public TManagedFrameOperator<TByteFrameBuffer>
{
protected:

   //
   // Input/Source: We get this from the preceding operator
   //
   TOperator<SOURCEFRAME> *             pSource ;
   TDataQueue<SOURCEFRAME> *            pSourceQueue ;

   //
   // These are the frames we will compute the deviation over.
   //
   TTemporalDataQueue<TByteFrameBuffer> tempFrameQueue ;

   //
   // We maintain one frame for the sum and the sum of squares
   // to efficiently compute the variation over.
   //
   struct THelperPixel {
      unsigned long sum ;
   };

   TFrameBuffer<THelperPixel>           helperFrame ;

   //
   // Output: The output frames are part of TOperator.
   //

public:

   TTemporalMotionFilter( TOperator<SOURCEFRAME> * pSource,
                          size_t N,
                          size_t tempN,
                          unsigned int width,
                          unsigned int height )
      : TManagedFrameOperator<TByteFrameBuffer>( N, width, height ),
        pSource( pSource ),
        pSourceQueue( pSource->getDataQueue() ),
        tempFrameQueue( tempN ),
        helperFrame( width, height )
   {
      //
      // Get frames to keep luminance information of the past tempN frames
      //
      for( unsigned int i = 0; i < tempN; i ++ )
      {
         TByteFrameBuffer *pBB ;
         try
         {
            pBB = new TByteFrameBuffer( width, height ); ;
         }
         catch(std::bad_alloc& badAlloc)
         {
            XI_OOM_HARD;
         }

         tempFrameQueue.swizzleWriterData( pBB ) ;

         //
         // The following method is somewhat overkill since it also deals with all kinds of
         // serialization issues. This is not necessary as this queue is written and read only from
         // within the operator thread.
         //
         tempFrameQueue.advanceWriterDataSlot() ;
      }

      //
      // Get one helper frame to cache some useful terms needed to incrementally maintain
      // the variance as we slide over frames.
      //

      //
      // At this point we can start grabbing frames from source to do some useful stuff.
      //
   }

   virtual ~TTemporalMotionFilter()
   {
      for( ;
           tempFrameQueue.getWriterData() != NULL ;
           tempFrameQueue.advanceWriterDataSlot() )
      {
         delete tempFrameQueue.getWriterData() ;

         tempFrameQueue.swizzleWriterData( NULL ) ;
      }
   }

   virtual void eventLoop()
   {
      unsigned int numProcessed = 0;
      BOOST_DEDUCED_TYPENAME TDataQueue<SOURCEFRAME>::TSubscription subscription ;

      unsigned int                 tempN = this->tempFrameQueue.getN() ;
      std::vector<unsigned char *> apTempPixels( tempN ) ;
      THelperPixel *               pHelperPixel ;

      while( !boost::this_thread::interruption_requested() )
      {
         try
         {
            pSourceQueue->renew( subscription ) ;
         }
         catch( boost::thread_interrupted interrupt )
         {
            break ;
         }

         //
         // Transfer image data over into the this frame - also update the helper frame
         //
         {
            TByteFrameBuffer * pBB ;
            unsigned char *    pDestPixel ;

            PIXELADAPTER pixelAdapter( subscription.pRep );

            pHelperPixel = this->helperFrame.getBuffer() ;

            pBB = tempFrameQueue.getWriterData() ;
            pDestPixel = pBB->getBuffer() ;

            for( unsigned int y = 0;
                     y < pBB->getHeight();
                     y ++ )
            {
               for( unsigned int x = 0;
                        x < pBB->getWidth(); )
               {
                  unsigned char pixelsInMacroPixel[PIXELADAPTER::getPixelsPerMacroPixel()];

                  pixelAdapter.toTarget( pixelsInMacroPixel ) ;

                  for( unsigned int m = 0;
                           m < PIXELADAPTER::getPixelsPerMacroPixel();
                           m ++, x ++)
                  {
                     if( numProcessed < tempN )
                     {
                        if( numProcessed == 0 )
                        {
                           //
                           // bootstrapping if we haven't processed any frames yet
                           //
                           pHelperPixel->sum = pixelsInMacroPixel[m] ;
                        }
                        else
                        {
                           //
                           // bootstrapping if we haven't processed tempN frames yet
                           //
                           pHelperPixel->sum += pixelsInMacroPixel[m] ;
                        }
                     }
                     else
                     {
                        //
                        // non-bootstrapping
                        //

                        //
                        // subtract oldest frame - i.e. the one we are going to write into in a moment
                        //
                        pHelperPixel->sum -= ( *pDestPixel ) ;

                        //
                        // add new frame
                        //
                        pHelperPixel->sum += pixelsInMacroPixel[m] ;
                     }

                     *pDestPixel = pixelsInMacroPixel[m] ;

                     pHelperPixel ++ ;
                     pDestPixel ++ ;
                  }

                  pixelAdapter++ ;
               }
            }

            //
            // Write out the temp frame - make sure we catch a possible interrupt
            // so we can give up the source subscription before we exit this method.
            //
            try
            {
               tempFrameQueue.writerDataFinishAndAdvance() ;
            }
            catch( boost::thread_interrupted interrupt )
            {
               break ;
            }

         }

         //
         // Now compute the output frame
         //
         if( numProcessed == tempN )
         {
            unsigned int    i;
            unsigned char * pDestPixel = this->frameQueue.getWriterData()->getBuffer() ;

            pHelperPixel = this->helperFrame.getBuffer() ;

            for( i = 0; i < tempN; i ++ )
            {
               apTempPixels[ i ] = this->tempFrameQueue[ i ]->getBuffer() ;
            }

            for( unsigned int y = 0;
                     y < this->helperFrame.getHeight();
                     y ++ )
            {
               for( unsigned int x = 0;
                        x < this->helperFrame.getWidth();
                        x ++ )
               {
                  double mean = (double)pHelperPixel->sum / (double)this->tempFrameQueue.getN() ;
                  double sumOfSquares = 0 ;

                  for( i = 0; i < tempN; i ++ )
                  {
                     sumOfSquares += (double)( tempN - i ) / ((double)( tempN ) * (double)( tempN ) ) * ( *apTempPixels[ i ] - mean ) * ( *apTempPixels[ i ] - mean );
                  }

                  *pDestPixel = (unsigned char )
                           pow( 2 * (double)sumOfSquares / ( (double)tempN ), (double)0.5 ) ;

                  pDestPixel ++ ;
                  pHelperPixel ++ ;

                  for( i = 0; i < tempN; i ++ )
                  {
                     apTempPixels[ i ] ++ ;
                  }
               }
            }
         }

         //
         // Write out the temp frame - make sure we catch a possible interrupt
         // so we can give up the source subscription before we exit this method.
         //
         try
         {
            this->frameQueue.writerDataFinishAndAdvance() ;
         }
         catch( boost::thread_interrupted interrupt )
         {
            break ;
         }

         if( numProcessed < this->tempFrameQueue.getN() )
         {
            numProcessed ++ ;
         }
      }

      if( subscription.pRep != NULL )
      {
         pSourceQueue->unsubscribe( subscription ) ;
      }
   }

};

template<class SOURCEFRAME, class PIXELADAPTER>
class TAverageMotionFilter : public TManagedFrameOperator<TByteFrameBuffer>
{
protected:

   //
   // Input/Source: We get this from the preceding operator
   //
   TOperator<SOURCEFRAME> *     pSource ;
   TDataQueue<SOURCEFRAME> *    pSourceQueue ;

   //
   // We maintain one frame for the sum and the sum of squares
   // to efficiently compute the variation over.
   //
   struct THelperPixel {
      double decayingAverage ;
   };

   TFrameBuffer<THelperPixel>   helperFrame ;

   //
   // Output: The output frames are part of TOperator.
   //

   double alpha ;

public:

   TAverageMotionFilter( TOperator<SOURCEFRAME> * pSource,
                         size_t N,
                         unsigned int width,
                         unsigned int height,
                         double alpha = 0.5 )
      : TManagedFrameOperator<TByteFrameBuffer>( N, width, height ),
        pSource( pSource ),
        pSourceQueue( pSource->getDataQueue() ),
        helperFrame( width, height ),
        alpha( alpha )
   { };

   virtual void eventLoop()
   {
      unsigned int numProcessed = 0;
      BOOST_DEDUCED_TYPENAME TDataQueue<SOURCEFRAME>::TSubscription subscription ;

      THelperPixel *pHelperPixel ;

      while( !boost::this_thread::interruption_requested() )
      {
         try
         {
            pSourceQueue->renew( subscription ) ;
         }
         catch( boost::thread_interrupted interrupt )
         {
            break ;
         }

         //
         // Transfer image data over into the next frame - also update the helper frame
         //
         {
            TByteFrameBuffer * pBB ;
            unsigned char *    pDestPixel ;

            PIXELADAPTER pixelAdapter( subscription.pRep );

            pHelperPixel = this->helperFrame.getBuffer() ;

            pBB = frameQueue.getWriterData() ;
            pDestPixel = pBB->getBuffer() ;

            for( unsigned int y = 0;
                     y < pBB->getHeight();
                     y ++ )
            {
               for( unsigned int x = 0;
                        x < pBB->getWidth(); )
               {
                  unsigned char pixelsInMacroPixel[PIXELADAPTER::getPixelsPerMacroPixel()];

                  pixelAdapter.toTarget( pixelsInMacroPixel ) ;

                  for( unsigned int m = 0;
                           m < PIXELADAPTER::getPixelsPerMacroPixel();
                           m ++, x ++)
                  {
                     if( numProcessed < 1 )
                     {
                        if( numProcessed == 0 )
                        {
                           //
                           // bootstrapping if we haven't processed any frames yet
                           //
                           *pDestPixel = pixelsInMacroPixel[m] ;
                           pHelperPixel->decayingAverage = (double)pixelsInMacroPixel[m] ;
                        }
                     }
                     else
                     {
                        //
                        // non-bootstrapping
                        //
                        *pDestPixel = (unsigned char)
                                 //(std::fabs( pixelsInMacroPixel[m] - pHelperPixel->decayingAverage ) /255 * pixelsInMacroPixel[m]);
                                   std::fabs( pixelsInMacroPixel[m] - pHelperPixel->decayingAverage ) ;

                        pHelperPixel->decayingAverage = alpha * (double)pixelsInMacroPixel[m] +
                                 ( 1 - alpha) * pHelperPixel->decayingAverage ;
                     }

                     pHelperPixel ++ ;
                     pDestPixel ++ ;
                  }

                  pixelAdapter++ ;
               }
            }
         }

         //
         // Write out the temp frame - make sure we catch a possible interrupt
         // so we can give up the source subscription before we exit this method.
         //
         try
         {
            this->frameQueue.writerDataFinishAndAdvance() ;
         }
         catch( boost::thread_interrupted interrupt )
         {
            break ;
         }

         if( numProcessed < 1 )
         {
            numProcessed ++ ;
         }
      }

      if( subscription.pRep != NULL )
      {
         pSourceQueue->unsubscribe( subscription ) ;
      }
   }

};

}}

#endif /* MOTION_H_ */
